#include <math.h>
void Jacobin(const double* q, const double* a, const double* d, double* J_p, double* J_r)
{
    double s1 = sin(q[0]);
    double s2 = sin(q[1]);
    double s3 = sin(q[2]);
    double s4 = sin(q[3]);
    double s5 = sin(q[4]);
    double c1 = cos(q[0]);
    double c2 = cos(q[1]);
    double c3 = cos(q[2]);
    double c4 = cos(q[3]);
    double c5 = cos(q[4]);
    //double J_p[9] = { 0.0 };
    /*double J_r[9] = { 0.0 };*/

    J_p[0] = c1 * d[3] - d[4] * (c4 * (c2 * s1 * s3 + c3 * s1 * s2) + s4 * (c2 * c3 * s1 - s1 * s2 * s3)) +
             d[5] * (c1 * c5 + s5 * (c4 * (c2 * c3 * s1 - s1 * s2 * s3) - s4 * (c2 * s1 * s3 + c3 * s1 * s2))) - a[3] * (c2 * c3 * s1 - s1 * s2 * s3) -
             a[2] * c2 * s1;
    J_p[1] = d[4] * (c4 * (c1 * c2 * c3 - c1 * s2 * s3) - s4 * (c1 * c2 * s3 + c1 * c3 * s2)) - a[3] * (c1 * c2 * s3 + c1 * c3 * s2) +
             d[5] * s5 * (c4 * (c1 * c2 * s3 + c1 * c3 * s2) + s4 * (c1 * c2 * c3 - c1 * s2 * s3)) - a[2] * c1 * s2;
    J_p[2] = d[4] * (c4 * (c1 * c2 * c3 - c1 * s2 * s3) - s4 * (c1 * c2 * s3 + c1 * c3 * s2)) - a[3] * (c1 * c2 * s3 + c1 * c3 * s2) +
             d[5] * s5 * (c4 * (c1 * c2 * s3 + c1 * c3 * s2) + s4 * (c1 * c2 * c3 - c1 * s2 * s3));
    J_p[3] = d[3] * s1 + d[5] * (c5 * s1 - s5 * (c4 * (c1 * c2 * c3 - c1 * s2 * s3) - s4 * (c1 * c2 * s3 + c1 * c3 * s2))) +
             a[3] * (c1 * c2 * c3 - c1 * s2 * s3) + d[4] * (c4 * (c1 * c2 * s3 + c1 * c3 * s2) + s4 * (c1 * c2 * c3 - c1 * s2 * s3)) + a[2] * c1 * c2;
    J_p[4] = d[4] * (c4 * (c2 * c3 * s1 - s1 * s2 * s3) - s4 * (c2 * s1 * s3 + c3 * s1 * s2)) - a[3] * (c2 * s1 * s3 + c3 * s1 * s2) +
             d[5] * s5 * (c4 * (c2 * s1 * s3 + c3 * s1 * s2) + s4 * (c2 * c3 * s1 - s1 * s2 * s3)) - a[2] * s1 * s2;
    J_p[5] = d[4] * (c4 * (c2 * c3 * s1 - s1 * s2 * s3) - s4 * (c2 * s1 * s3 + c3 * s1 * s2)) - a[3] * (c2 * s1 * s3 + c3 * s1 * s2) +
             d[5] * s5 * (c4 * (c2 * s1 * s3 + c3 * s1 * s2) + s4 * (c2 * c3 * s1 - s1 * s2 * s3));
    J_p[6] = 0;
    J_p[7] = a[2] * c2 + d[4] * (c4 * (c2 * s3 + c3 * s2) + s4 * (c2 * c3 - s2 * s3)) + a[3] * (c2 * c3 - s2 * s3) -
             d[5] * s5 * (c4 * (c2 * c3 - s2 * s3) - s4 * (c2 * s3 + c3 * s2));
    J_p[8] = d[4] * (c4 * (c2 * s3 + c3 * s2) + s4 * (c2 * c3 - s2 * s3)) + a[3] * (c2 * c3 - s2 * s3) -
             d[5] * s5 * (c4 * (c2 * c3 - s2 * s3) - s4 * (c2 * s3 + c3 * s2));

    J_r[0] = s1;
    J_r[1] = c4 * (c1 * c2 * s3 + c1 * c3 * s2) + s4 * (c1 * c2 * c3 - c1 * s2 * s3);
    J_r[2] = c5 * s1 - s5 * (c4 * (c1 * c2 * c3 - c1 * s2 * s3) - s4 * (c1 * c2 * s3 + c1 * c3 * s2));
    J_r[3] = -c1;
    J_r[4] = c4 * (c2 * s1 * s3 + c3 * s1 * s2) + s4 * (c2 * c3 * s1 - s1 * s2 * s3);
    J_r[5] = -c1 * c5 - s5 * (c4 * (c2 * c3 * s1 - s1 * s2 * s3) - s4 * (c2 * s1 * s3 + c3 * s1 * s2));
    J_r[6] = 0;
    J_r[7] = s4 * (c2 * s3 + c3 * s2) - c4 * (c2 * c3 - s2 * s3);
    J_r[8] = -s5 * (c4 * (c2 * s3 + c3 * s2) + s4 * (c2 * c3 - s2 * s3));
}

void Mmt(const double* A, double* B)
{
    B[0] = A[0] * A[0] + A[1] * A[1] + A[2] * A[2];
    B[1] = A[0] * A[3] + A[1] * A[4] + A[2] * A[5];
    B[2] = A[0] * A[6] + A[1] * A[7] + A[2] * A[8];
    B[3] = A[3] * A[0] + A[4] * A[1] + A[5] * A[2];
    B[4] = A[3] * A[3] + A[4] * A[4] + A[5] * A[5];
    B[5] = A[3] * A[6] + A[4] * A[7] + A[5] * A[8];
    B[6] = A[6] * A[0] + A[7] * A[1] + A[8] * A[2];
    B[7] = A[6] * A[3] + A[7] * A[4] + A[8] * A[5];
    B[8] = A[6] * A[6] + A[7] * A[7] + A[8] * A[8];
}

void Mmm(const double* A, double* B, double* C)
{
    C[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6];
    C[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7];
    C[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8];
    C[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6];
    C[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7];
    C[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8];
    C[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6];
    C[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7];
    C[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8];
}

void Mcond(const double* eigval, double* cond) { *cond = eigval[0] / eigval[2]; }

double Mnorm2(const double* A)
{
    double norm = 0.0;
    for (int i = 0; i < 9; i++) { norm += A[i] * A[i]; }
    return sqrt(norm);
}

double Dvec2(const double* a, const double* b)
{
    double normd = 0.0;
    normd = a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
    return normd;
}
void QRdecop(const double* A, double* Q, double* R)
{
    double a0[3] = {0};
    double a1[3] = {0};
    double a2[3] = {0};
    double b0[3] = {0};
    double b1[3] = {0};
    double b2[3] = {0};

    a0[0] = A[0];
    a0[1] = A[3];
    a0[2] = A[6];
    a1[0] = A[1];
    a1[1] = A[4];
    a1[2] = A[7];
    a2[0] = A[2];
    a2[1] = A[5];
    a2[2] = A[8];

    b0[0] = a0[0];
    b0[1] = a0[1];
    b0[2] = a0[2];
    double temp1 = Dvec2(a1, b0) / Dvec2(b0, b0);
    b1[0] = a1[0] - temp1 * b0[0];
    b1[1] = a1[1] - temp1 * b0[1];
    b1[2] = a1[2] - temp1 * b0[2];
    double temp2 = Dvec2(a2, b0) / Dvec2(b0, b0);
    double temp3 = Dvec2(a2, b1) / Dvec2(b1, b1);
    b2[0] = a2[0] - temp2 * b0[0] - temp3 * b1[0];
    b2[1] = a2[1] - temp2 * b0[1] - temp3 * b1[1];
    b2[2] = a2[2] - temp2 * b0[2] - temp3 * b1[2];

    double normb0 = sqrt(Dvec2(b0, b0));
    double normb1 = sqrt(Dvec2(b1, b1));
    double normb2 = sqrt(Dvec2(b2, b2));

    Q[0] = b0[0] / normb0;
    Q[1] = b1[0] / normb1;
    Q[2] = b2[0] / normb2;
    Q[3] = b0[1] / normb0;
    Q[4] = b1[1] / normb1;
    Q[5] = b2[1] / normb2;
    Q[6] = b0[2] / normb0;
    Q[7] = b1[2] / normb1;
    Q[8] = b2[2] / normb2;

    double tempM[9] = {0};
    tempM[0] = 1.0;
    tempM[1] = temp1;
    tempM[4] = 1.0;
    tempM[2] = temp2;
    tempM[5] = temp3;
    tempM[8] = 1.0;

    R[0] = tempM[0] * normb0;
    R[1] = tempM[1] * normb0;
    R[2] = tempM[2] * normb0;
    R[4] = tempM[4] * normb1;
    R[5] = tempM[5] * normb1;
    R[8] = tempM[8] * normb2;
}

void sorteig(double* A)
{
    for (int i = 0; i < 3; i++) { A[i] = fabs(A[i]); }
    double temp = 0.0;
    for (int i = 0; i < 2; i++)
    {
        for (int j = 0; j < 2 - i; j++)
        {
            if (A[j] > A[j + 1])
            {
                temp = A[j];
                A[j] = A[j + 1];
                A[j + 1] = temp;
            }
        }
    }
}
